#include "yaml_to_2d_table.h"

namespace auto_ros
{
namespace control
{
bool yaml_to_2d_table(std::string engine_map_yaml_path_file, Interpolation2D &map_2d)
{
	Interpolation2D::DataType xyz;
	YAML::Node map_yaml_node = YAML::LoadFile(engine_map_yaml_path_file); //也可以这样读取文件
	std::vector<double> acc_brake_percent_vec =
		map_yaml_node["acc_brake_percent"].as<std::vector<double>>();
	std::vector<double> speed_vec = map_yaml_node["speed"].as<std::vector<double>>();
	int percent_num = acc_brake_percent_vec.size();
	int speed_num = speed_vec.size();
	int map_rows = map_yaml_node["map"].size();
	if (speed_num != map_rows)
	{
		std::cerr << "\033[31m speed_num != map_row  \033[0m" << std::endl;
		return false;
	}
	for (int row_index = 0; row_index < map_rows; row_index++)
	{
		std::vector<double> temp_row_vec = map_yaml_node["map"][row_index].as<std::vector<double>>();
		if (temp_row_vec.size() != percent_num)
		{
			std::cerr << "\033[31m temp_row_vec.size != percent_num  \033[0m" << std::endl;
			return false;
		}
		for (int col_index = 0; col_index < temp_row_vec.size(); col_index++)
		{
			auto temp_tuple = std::make_tuple(
				speed_vec[row_index], temp_row_vec[col_index], acc_brake_percent_vec[col_index]);
			xyz.push_back(temp_tuple);
		}
	}
	if (map_2d.Init(xyz) == false)
	{
		std::cerr << "\033[31m map_2d.Init(xyz) = false \033[0m" << std::endl;
		return false;
	};
	return true;
}
} // namespace control
} // namespace auto_ros
